COMMENT ⊗   VALID 00013 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	TITLE ARMSOL
C00004 00003	Overall declarations
C00005 00004	BEGIN CROSS
C00006 00005	BEGIN SECOND
C00007 00006	BEGIN ANGLE
C00009 00007	BEGIN HANDSOL
C00011 00008	BEGIN ARMSOL 
C00017 00009		ARMSOL cont
C00019 00010		ARMSOL cont
C00022 00011		ARMSOL cont
C00023 00012	Data and local variables
C00026 00013	BUGS
C00027 ENDMK
C⊗;
TITLE ARMSOL
COMMENT ⊗

Written by LOU
Modified by RF 2/11/75:

The purpose of this set of routines is solely to be used as an armsol
for the HAL compiler.  There is a general mechanism for finding
solutions for any arm; the correct call is 

FLAG ← ARMSOL(RESULT,ARMNUMBER,ROT,LOC)

where RESULT is the first array location where the result is to be
put (out of 6 locations), ARMNUMBER is 0 for yellow, 1 for blue, ROT,
a 3x3, is the rotation part of the frame to be solved, stored by
columns, and LOC is the first location of a 3-vector, the location
part of the frame.  If the result is out of range, FLAG will be TRUE,
and RESULT will be at the stop limit.  
⊗
;Overall declarations

	INTERNAL ARMSOL,HANDSOL,LOSTOP,HISTOP,TIMFAC
	EXTERNAL ACOS,SQRT,ATAN2,ASIN,ACOS,SIND,COSD

	↓AC←1
	↓P←17

	S22V←←36.6025

↓RAD:	206712273406
BEGIN CROSS

;CROSS(R,P,A) takes the 3-vecotrs P and A and returns their
;cross-product in R. 
;TIME APPROX 180 MICRO SECONDS

	TAC1←←2
	PV1←←3
	PV2←←4
	PV3←←5
	AV1←←6
	AV2←←7
	AV3←←10
	O←←11

↑CROSS:
	HRL AC,-2(P)		;MOVE PV INTO AC'S
	HRRI AC,PV1
	BLT AC,PV3
	HRL AC,-1(P)		;MOVE AV INTO AC'S
	HRRI AC,AV1
	BLT AC,AV3
	MOVE O,-3(P)		
	MOVE AC,PV2
	FMPR AC,AV3
	MOVE TAC1,PV3
	FMPR TAC1,AV2
	FSBR AC,TAC1
	MOVEM AC,(O)
	FMPR PV3,AV1
	FMPR AV3,PV1
	FSBR PV3,AV3
	MOVEM PV3,1(O)
	FMPR PV1,AV2
	FMPR AV1,PV2
	FSBR PV1,AV1
	MOVEM PV1,2(O)
	SUB P,[4(4)]
	JRST @4(P)
	BEND

BEGIN SECOND

;SECOND(A) returns the square of the magnitude of 3-vector A in
;register 0.
;TIME APPROX 90 MICRO SEC

	A←←4

↑SECOND:
	HRL A,-1(P)
	HRRI A,AC
	BLT A,AC+2
	FMPR AC,AC
	FMPR AC+1,AC+1
	FMPR AC+2,AC+2
	FADR AC,AC+1
	FADR AC,AC+2
	SUB P,[2(2)]
	JRST @2(P)
	BEND

BEGIN ANGLE

;ANGLE(P,A,O) RETURNS THE ANGLE BETWEEN P AND A IN DEGREES ABOUT O
;A AND P MUST BE UNIT VECTORS. I.E. DIRECTION COSINES
;COMPUTES THE ANGLE=ACOS(PV1*AV1 + PV2*AV2 + PV3*AV3)
;AND THEN THE DETERMINANT
;	| O1  O2  O3|
;	|PV1 PV2 PV3|
;	|AV1 AV2 AV3|
;AND IF POSITIVE MULTIPLIES THE ANGLE BY -1;
;TIME APPROX 550 MICRO SECONDS

	TAC1←←2
	PV1←←3
	PV2←←4
	PV3←←5
	AV1←←6
	AV2←←7
	AV3←←10
	O←←11
	I←←13			;12 IS A NO NO

↑ANGLE:
	HRL AC,-3(P)		;MOVE PV INTO AC'S
	HRRI AC,PV1
	BLT AC,PV3
	HRL AC,-2(P)		;MOVE AV INTO AC'S
	HRRI AC,AV1
	BLT AC,AV3
	MOVEI I,2		;COMPUTE DOT PRODUCT
	MOVE AC,PV1
	FMPR AC,AV1
L1:	MOVE TAC1,PV1(I)
	FMPR TAC1,AV1(I)
	FADR AC,TAC1
	SOJG I,L1
	MOVEM AC,DOT
	MOVE O,-1(P)		;COMPUTE DETERMINANT
	MOVE AC,PV2
	FMPR AC,AV3
	MOVE TAC1,PV3
	FMPR TAC1,AV2
	FSBR AC,TAC1
	FMPR AC,(O)
	FMPR PV3,AV1
	FMPR AV3,PV1
	FSBR PV3,AV3
	FMPR PV3,1(O)
	FADR AC,PV3
	FMPR PV1,AV2
	FMPR AV1,PV2
	FSBR PV1,AV1
	FMPR PV1,2(O)
	FADR AC,PV1
	MOVEM AC,DET
	PUSH P,DOT
	PUSHJ P,ACOS
	FMPR AC,RAD
	SKIPL DET		;NEGATE ANGLE IF DET NEGATIVE
	MOVN AC,AC
	SUB P,[4(4)]
	JRST @4(P)
DOT:	0
DET:	0
	BEND

BEGIN HANDSOL

COMMENT ⊗

FLAG ← HANDSOL(RESULT,JOINTNUMBER,OPENING)

where RESULT is the location to put the result, JOINTNUMBER is
7 for the yellow hand and 14 for the blue hand, and OPENING is
the desired opening.  If OPENING is out of range, FLAG will be
FALSE and RESULT will be the stop limit.
⊗

	TAC ←← 2
	RESULT ←← -3
	JOINTNUMBER ←← -2
	OPENING ←← -1

↑HANDSOL:
	SETOM FLAG		;True
	MOVE AC,OPENING(P)
	MOVE TAC,JOINTNUMBER(P)
	SOJ TAC,
	CAMLE AC,HISTOP(TAC)
	JRST[	MOVE AC,HISTOP(TAC)
		JRST M1]
	CAMGE AC,LOSTOP(TAC)
	JRST[	MOVE AC,LOSTOP(TAC)
	M1:	SETZM FLAG	;WAS OUT OF RANGE
		JRST OK1]
OK1:	MOVEM AC,@RESULT(P)

	MOVE AC,FLAG		;Put flag in AC for return
	SUB P,[4(4)]
	JRST @4(P)		;Return

	BEND
BEGIN ARMSOL 

COMMENT ⊗

FLAG ← ARMSOL(RESULT,ARMNUMBER,ROT,LOC)

where RESULT is the first array location where the result is to be
put (out of 6 locations), ARMNUMBER is 0 for yellow, 1 for blue, ROT,
a 3x3, is the rotation part of the frame to be solved, stored by
columns, and LOC is the first location of a 3-vector, the location
part of the frame.  If the result is out of range, FLAG will be FALSE,
and RESULT will be at the stop limit.  

The time required is about 4 milliseconds.
⊗

	TAC←←2
	I←←3
	RESULT ←← -4
	ARMNUMBER ←← -3
	ROT ←← -2
	LOC ←← -1

↑ARMSOL:
;Initialization:
	SETOM FLAG		;SET RETURN FLAG TRUE
;	HRL AC,RESULT(P)	;First guess at answer is taken from RESULT
;	HRRI AC,J
;	BLT AC,J+5
;	Temporarily just use previous answer as the first guess.  This is
;	only used for joint 4, anyway.
	MOVE AC,ARMNUMBER(P)	;Armnumber
	MOVE TAC,S2V(AC)	;Pick up correct S2V
	MOVEM TAC,S2		;Put in position.
	MOVE TAC,S6V(AC)	;Pick up correct S6V
	MOVEM TAC,S6		;Put in position.
	MOVE TAC,RHS(AC)	;Pick up correct RH
	MOVEM TAC,RH		;Put in position.
	IMULI AC,3		;3 sholder numbers for each arm
	HRLZI TAC,SHOLS(AC)	;
	HRRI TAC,SHOLDER	;
	BLT TAC,SHOLDER+2	;BLT the sholder nos. into position.
	MOVE AC,ARMNUMBER(P)	;
	IMULI AC,7		;7 low, high stops for each arm.
	HRLZI TAC,LOSTOP(AC)	;
	HRRI TAC,LSTOP		;
	BLT TAC,LSTOP+6		;BLT the low stops into position.
	HRLZI TAC,HISTOP(AC)	;
	HRRI TAC,HSTOP		;
	BLT TAC,HSTOP+6		;BLT the high stops into position.
;COMPUTE W=P-SHOLDER-A*S6
;RF:  Modified for separate ROT, LOC.  
;Move the arguments to local store.
	HRLZ AC,LOC(P)		;First word of source
	HRRI AC,PV1		;First word of destination
	BLT AC,PV1+2		;BLT the LOC into PV
	MOVE TAC,ROT(P)		;
	HRLZI AC,3(TAC)		;First word of source
	HRRI AC,OV1		;First word of destination
	BLT AC,AV1+2		;BLT the ROT into OV, AV.
;J2=DOT(W,W)-S2↑2
	MOVN TAC,S22		;INITIALIZE TAC TO -S22
	MOVEI I,2		;COMPUTE THE DOT PRODUCT
L1:	MOVE AC,AV1(I)
	FMPR AC,S6
	FADR AC,SHOLDER(I)
	FSBR AC,PV1(I)
	MOVEM AC,W(I)		;-W(I)
	FMPR AC,AC
	MOVEM AC,W2(I)		;W(I)↑2
	FADR TAC,AC
	SOJGE I,L1
	JUMPLE TAC,[SETZ AC,
		JRST NSR]
	PUSH P,TAC
	PUSHJ P,SQRT
NSR:	CAMLE AC,HSTOP+2		;CHECK THE STOPS FOR JOINT 3
	JRST[	MOVE AC,HSTOP+2
		JRST M3]
	CAMGE AC,LSTOP+2
	JRST[	MOVE AC,LSTOP+2
	M3:	SETZM FLAG	;WAS OUT OF RANGE.
		JRST OK3]
OK3:	MOVEM AC,J+2		;JOINT THREE

;NOW COMPUTE THE BOOM OFFSET
	MOVE AC,W2+1
	FADR AC,W2
	PUSH P,AC
	PUSHJ P,SQRT
	MOVM TAC,S2
	FDVR TAC,AC
	CAML TAC,[1.0]
	MOVSI TAC,(1.0)
	PUSH P,TAC
	PUSHJ P,ASIN
	MOVEM AC,JTEMP

;NOW COMPUTE TH FOR JOINT 1
	PUSH P,W+1
	PUSH P,W
	PUSHJ P,ATAN2
	HRRZ TAC,RH		;Which hand?
	JUMPE TAC,LEFTH		;Left hand.
	FADR AC,JTEMP		;Right.  Add J.
	FMPR AC,RAD
	MOVE TAC,AC
	FSBR TAC,[90.0]
	JUMPL TAC,ISOK
	FSBR AC,[360.0]
	JRST ISOK
LEFTH:	FSBR AC,JTEMP		;Left.  Subtract J.
	FMPR AC,RAD
	MOVE TAC,AC
	FADR TAC,[90.0]
	JUMPG TAC,ISOK
	FADR AC,[360.0]
ISOK:	CAMLE AC,HSTOP		;AND CHECK THE STOPS FOR JOINT 1
	JRST[	MOVE AC,HSTOP
		JRST M1]
	CAMGE AC,LSTOP
	JRST[	MOVE AC,LSTOP
	M1:	SETZM FLAG	;WAS OUT OF RANGE
		JRST OK1]
OK1:	MOVEM AC,J		;JOINT ONE
;JOINT 2-ACOS(W3/J3)
	MOVN AC,W+2
	FDVR AC,J+2
	MOVEM AC,CO2
	PUSH P,AC
	PUSHJ P,ACOS
	FMPR AC,RAD
	MOVNM AC,J+1		;JOINT TWO
	;ARMSOL cont

;WE NOW NEED SOME SINES AND COSINES
	PUSH P,J
	PUSHJ P,SIND
	MOVEM AC,SI1
	PUSH P,J
	PUSHJ P,COSD
	MOVEM AC,CO1
	PUSH P,J+1
	PUSHJ P,SIND
	MOVEM AC,SI2
;WE NOW COMPUTE THE UNIT VECTORS AT THE END OF LINK 3
	MOVE AC,CO1
	FMPR AC,CO2
	MOVEM AC,Y
	MOVE AC,SI1
	FMPR AC,CO2
	MOVEM AC,Y+1
	MOVE AC,SI2
	MOVNM AC,Y+2
	FMPR AC,CO1
	MOVEM AC,Z
	MOVE AC,SI1
	FMPR AC,SI2
	MOVEM AC,Z+1
	MOVE AC,CO2
	MOVEM AC,Z+2
;CROSS(VT,Z,A);
	PUSH P,[VT]
	PUSH P,[Z]
	PUSH P,[AV1]
	PUSHJ P,CROSS
;CHECK FOR DEGENERATE CASE
	PUSH P,[VT]
	PUSHJ P,SECOND
	CAMG AC,[0.000001]
	JRST DEGEN		;JOINTS 4 AND 6 ARE DEGENERATE
	PUSH P,AC
	PUSHJ P,SQRT		;GET MAGNITUDE OF VT
	MOVEI I,2
FVT:	MOVE TAC,VT(I)
	FDVR TAC,AC
	MOVEM TAC,VT(I)
	SOJGE I,FVT
VTF:				;VT NOW UNIT MAGNITUDE
	;ARMSOL cont

;NOW JOINT 4
	PUSH P,[VT]
	PUSH P,[Y]
	PUSH P,[Z]
	PUSHJ P,ANGLE
	MOVE TAC,AC
	FSBR TAC,J+3		;Compare with previous answer for joint 4
	CAMLE TAC,[180.0]
	JRST[	FSBR AC,[360.0]
		JRST TN]
	CAMGE TAC,[-180.0]
	JRST[	FADR AC,[360.0]
		JRST TN]
TN:
	MOVE TAC,AC
	FSBR TAC,J+3
	CAMLE TAC,[90.0]
	JRST[	FSBR AC,[180.0]
		JRST RVTO]
	CAMGE TAC,[-90.0]
	JRST[	FADR AC,[180.0]
	RVTO:	MOVNS VT
		MOVNS VT+1
		MOVNS VT+2
		JRST SOK4]
SOK4:
;NOW CHECK THE STOPS FOR JOINT 4
	CAMLE AC,HSTOP+3
	JRST[	FSBR AC,[180.0]
		JRST RVT]
	CAMGE AC,LSTOP+3
	JRST[	FADR AC,[180.0]
	RVT:	MOVNS VT
		MOVNS VT+1
		MOVNS VT+2
		JRST OK4]
OK4:	MOVEM AC,J+3		;JOINT FOUR
;NOW JOINT 5
	PUSH P,[AV1]
	PUSH P,[Z]
	PUSH P,[VT]
	PUSHJ P,ANGLE
	CAMLE AC,HSTOP+4
	JRST[	MOVE AC,HSTOP+4
		JRST M5]
	CAMGE AC,LSTOP+4
	JRST[	MOVE AC,LSTOP+4
	M5:	SETZM FLAG	;WAS OUT OF RANGE
		JRST OK5]
OK5:	MOVEM AC,J+4
;JOINT 6 IS NOW SIMPLE
	PUSH P,[OV1]
	PUSH P,[VT]
	PUSH P,[AV1]
	PUSHJ P,ANGLE
TOL:	MOVE TAC,AC
	FSBR TAC,J+5
	CAML TAC,[180.0]
	JRST[	FSBR AC,[360.0]
		JRST ST6]
	CAMGE TAC,[-180.0]
	JRST[	FADR AC,[360.0]
		JRST ST6]
ST6:	CAMLE AC,HSTOP+5
	JRST[	FSBR AC,[360.0]
		JRST OK6]
	CAMGE AC,LSTOP+5
	FADR AC,[360.0]
OK6:	MOVEM AC,J+5

;Store away the answer
	HRRZ TAC,RESULT(P)
	HRRZ AC,TAC
	HRLI AC,J
	BLT AC,5(TAC)

	MOVE AC,FLAG		;Put flag in AC for return
	SUB P,[5(5)]
	JRST @5(P)		;Return

	;ARMSOL cont

DEGEN:	SETZM J+4		;JOINT 5 IS ZERO
	PUSH P,[OV1]
	PUSH P,[Y]
	PUSH P,[AV1]
	PUSHJ P,ANGLE
	FSBR AC,J+3		;LEAVE JOINT 4 AS IS
	JRST TOL		;AND PROCEED AS USUAL


	BEND


;Data and local variables

; Arm-independent
FLAG:	0
JTEMP:	0
OV1:	BLOCK 3			;The rotation is stored in
AV1:	BLOCK 3			; these 6 places.  Keep in order.
PV1:	BLOCK 3			;The location is stored here.
W:	0
SI1:	0
CO1:	0
W2:	0
SI2:	0
CO2:	0
VT:	BLOCK 3
Y:	BLOCK 3
Z:	BLOCK 3
J:	BLOCK 6
S22:	S22V
TPI:	203622077325

; Arm-dependent
SHOLDER:BLOCK 3			;Initialized from SHOLS
SHOLS:	29.5  			;Yellow
	8.375 
	16.24
	29.53125		;Blue
	50.805
	20.24

S2:	0			;Initialized to S2V, depending on arm.
S2V:	6.05			;Yellow
	-6.05			;Blue

S6:	0			;Initialized to S6V, depending on arm.
	;Distance to center of fingers (not tip).
S6V:	9.38			;Yellow
	9.28125			;Blue

RH:	0			;Initialized from RHS
RHS:	1			;Yellow
	0			;Blue

LSTOP:	BLOCK 7
↑LOSTOP:
	-185.0			;Yellow arm
	-185.0
	6.5
	-175.0
	-101.0
	-270.0
	0.0			;Yellow fingers
	-90.0			;Blue arm
	-185.0
	6.2
	-321.0
	-100.0
	-158.5
	0.0			;Blue fingers

HSTOP:	BLOCK 7
↑HISTOP:
	60.0			;Yellow arm
	-10.0
	27.5
	140.0
	101.0
	270.0
	3.89			;Yellow fingers
	190.0			;Blue arm
	-60.0
	34.9
	236.0
	101.0
  	158.5
	3.89			;Blue fingers

;the following is used by TCALC, but belongs here.  It is the number of
;jiffies required by each joint to move one degree (or inch for joints 3,7)
↑TIMFAC:
	0.75			;Yellow arm
	0.5
	4.5
	0.2
	0.2
	0.4
	5.0			;Yellow fingers
	0.75			;Blue arm
	0.75
	4.5
	0.2
	0.2
	0.4
	5.0			;Blue fingers

	END
;BUGS

COMMENT ⊗

Initial joint 4 should be arm-dependent, not 0.  Causes wild initial
swivels.

BLUE:	180.
	-90
	14
	-90
	+90
	0
YELLOW:	-180
	-90
	12
	-90
	90
	0

THE OUTPUT AREA SHOULD BE 14 LONG.
⊗